#include "utils.h"
#include "preprocesamiento.h"
//Angulo en sexagesimal
double angleBetween( Point pt1, Point pt2, Point pt0 )
{
	double dx1 = pt1.x - pt0.x;
	double dy1 = pt1.y - pt0.y;
	double dx2 = pt2.x - pt0.x;
	double dy2 = pt2.y - pt0.y;
	return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

Point2f computeIntersect(Vec4i a, Vec4i b)  
{  
	int x1 = a[0], y1 = a[1], x2 = a[2], y2 = a[3];  
	int x3 = b[0], y3 = b[1], x4 = b[2], y4 = b[3];  
	if (x1 == 0 && x3 == 0) return Point2f(-1,-1); 
	if (y1 == 0 && y3 == 0) return Point2f(-1,-1);
	// Solo se computaran las intersecciones entre lineas horizontales y verticales
	if (float d = ((float)(x1-x2) * (y3-y4)) - ((y1-y2) * (x3-x4)))  
	{  
		Point2f pt;  
		pt.x = ((x1*y2 - y1*x2) * (x3-x4) - (x1-x2) * (x3*y4 - y3*x4)) / d;  
		pt.y = ((x1*y2 - y1*x2) * (y3-y4) - (y1-y2) * (x3*y4 - y3*x4)) / d; 
		/*
		//-20 is a threshold, the POI can be off by at most 20 pixels
		if(pt.x<min(x1,x2)-20||pt.x>max(x1,x2)+20||pt.y<min(y1,y2)-20||pt.y>max(y1,y2)+20){  
		return Point2f(-1,-1);  
		}  
		if(pt.x<min(x3,x4)-20||pt.x>max(x3,x4)+20||pt.y<min(y3,y4)-20||pt.y>max(y3,y4)+20){  
		return Point2f(-1,-1);  
		}  */
		return pt;  
	}  
	else  
		return Point2f(-1, -1);  
} 

bool comparator(Point2f a,Point2f b){  
	return a.x<b.x;  
}  
void sortCorners(vector<Point2f>& corners, Point2f center)  
{  
	vector<Point2f> top, bot;  
	for (size_t i = 0; i < corners.size(); i++)  
	{  
		if (corners[i].y < center.y)  
			top.push_back(corners[i]);  
		else  
			bot.push_back(corners[i]);  
	}  
	sort(top.begin(),top.end(),comparator);  
	sort(bot.begin(),bot.end(),comparator);  
	Point2f tl = top[0];
	Point2f tr = top[top.size()-1];
	Point2f bl = bot[0];
	Point2f br = bot[bot.size()-1];  
	corners.clear();  
	corners.push_back(tl);  
	corners.push_back(tr);  
	corners.push_back(br);  
	corners.push_back(bl);  
}  

int distanceBetween( Point pt1, Point pt2 ){
	int dx = pt1.x - pt2.x;
	int dy = pt1.y - pt2.y;
	double distance = sqrt(dx*dx + dy*dy);
	return (int)distance;
}

double distanceBetweenF( Point pt1, Point pt2 ){
	int dx = pt1.x - pt2.x;
	int dy = pt1.y - pt2.y;
	double distance = sqrt(dx*dx + dy*dy);
	return distance;
}

bool cumpleRatio(vector<Point2f> poligono,double valor, double error){
	double ancho = distanceBetweenF(poligono[0],poligono[1]);
	double alto = distanceBetweenF(poligono[0],poligono[3]);
	double ratioWH = ancho/alto;
	return ((ratioWH <= valor + error) && (ratioWH >= valor - error));
}

bool cumpleRatio(int w, int h,double valor, double errorNeg,double errorPos){
	double ancho = w;
	double alto = h;
	double ratioWH = ancho/alto;
	return ((ratioWH <= valor + errorPos) && (ratioWH >= valor - errorNeg));
}
bool esParalelogramo(vector<Point2f> poligono,double error){
	double anchoSuperior = distanceBetweenF(poligono[0],poligono[1]);
	double anchoInferior = distanceBetweenF(poligono[2],poligono[3]);
	double altoDerecho = distanceBetweenF(poligono[1],poligono[2]);
	double altoIzquierdo = distanceBetweenF(poligono[3],poligono[0]);

	double deltaAncho = abs(anchoSuperior - anchoInferior);
	double deltaAlto = abs(altoDerecho - altoIzquierdo);
	return ((deltaAncho < error) && (deltaAlto < error));
}

int * calcularHistograma(unsigned char* img_data, int w, int h){
	int * histograma = (int*)calloc(256,sizeof(int));
	for (size_t i = 0; i < w*h; i++){
		histograma[img_data[i]]++;
	}
	return histograma;
}
int * calcularSumaCortesHorizontales(Mat img){
	unsigned char* img_data = img.data;
	int w = img.cols;
	int h = img.rows;
	int * histograma = (int*)calloc(h,sizeof(int));
	for (size_t i = 0; i < h; i++){
		for (size_t j = 0; j < w; j++){
			histograma[i]+= img_data[i*w+j];
		}
	}
	return histograma;
}
int * calcularSumaCortesVerticales(Mat img){
	unsigned char* img_data = img.data;
	int w = img.cols;
	int h = img.rows;
	int * histograma = (int*)calloc(w,sizeof(int));
	for (size_t j = 0; j < w; j++){
		for (size_t i = 0; i < h; i++){
			int pixel = img_data[i*w+j];
			histograma[j]+= (pixel == 255 ? 0 : 1);
		}
	}
	return histograma;
}

Mat perspectiva(Mat src, vector<Point2f> corners,int rows,int cols,int k){
	Mat aux = src.clone();
	// Define the destination image
	Mat quad = Mat::zeros(rows, cols, CV_8UC3);

	// Corners of the destination image
	vector<Point2f> quad_pts;
	quad_pts.push_back(Point2f(0, 0));
	quad_pts.push_back(Point2f(quad.cols, 0));
	quad_pts.push_back(Point2f(quad.cols, quad.rows));
	quad_pts.push_back(Point2f(0, quad.rows));

	// Get transformation matrix
	Mat transmtx = getPerspectiveTransform(corners, quad_pts);

	// Apply perspective transformation
	warpPerspective(aux, quad, transmtx, quad.size());
	//char nombre[20];
	//sprintf(nombre,"zones/cuad%d.jpg",k);

	//cvtColor(quad, quad , CV_BGR2GRAY);
	//preprocesarPlaca(quad,quad);
	/*
	char crosscuts[30];
	int nCrosscuts = horizontalCrosscut(quad,rows/2);
	sprintf(crosscuts,"Cross%d",nCrosscuts);
	putText(quad, crosscuts, cvPoint(30,30),FONT_HERSHEY_COMPLEX_SMALL, 1, Scalar(127), 1, CV_AA);
	*/
	//imshow(nombre, quad);
	//imwrite(nombre,quad);
	return quad;
}

int horizontalCrosscut(Mat src,int row){
	int width = src.cols;
	unsigned char * data = src.data;
	int flagZona = 0;
	int pixel;
	int inicio = 0,fin = width-1;
	int nObjetos = 0;
	for (int i = 0; i < width; i++){
		pixel = data[row*width + i];
		if (pixel == 0){
			inicio = i;
			break;
		}
	}
	for (int i = width - 1; i < 0; i++){
		pixel = data[row*width + i];
		if (pixel == 0){
			fin = i;
			break;
		}
	}
	for (int i = inicio; i <=fin; i++){
		pixel = data[row*width + i];
		if (pixel != flagZona){
			if (flagZona == 0)
				nObjetos++;
			flagZona = pixel;
		}
	}
	return nObjetos;
}

int * derivadaHist(int* funcion,int length){
	int * derivada = (int*) calloc(length,sizeof(int));
	for (int i = 0; i < length - 1; i++){
		derivada[i] = funcion[i+1] - funcion[i];
	}
	derivada[length - 1] = funcion[length - 1];
	return derivada;
}

int * hallarTransiciones(int* funcion,int length){
	int * transiciones = (int*) calloc(length,sizeof(int));
	int puntero = funcion[0];
	for (int i = 0; i < length; i++){
		if (funcion[i] != puntero){
			if (puntero == 0)
				transiciones[i]++; //Si crece desde 0 es transicion positiva
			if (funcion[i] == 0)
				transiciones[i]--; //Si decrece a 0 es transicion negativa
		}
		puntero = funcion[i];
	}
	return transiciones;
}

int * hallarMaximosMitad(int* funcion,int length,int width){
	int * derivada = derivadaHist(funcion,length);
	int * maximosLocales = (int*) calloc(length,sizeof(int));
	int * maximos = (int*) calloc(2,sizeof(int));
	int puntero = derivada[0];
	for (int i = 0; i < length; i++){
		if (puntero >= 0 && derivada[i] < 0 && (puntero - derivada[i] > 0) ){
			maximosLocales[i]++;
		}
		puntero = derivada[i];
	}
	
	int * derivada2 = derivadaHist(derivada,length);
	free(derivada);
	int valMax = 0,lastMax = 0;
	int max = 0;

	for (int i=length/2-1; i > length/5 ; i--){
		if (maximosLocales[i]){
			lastMax = i;
		}
		if (valMax < maximosLocales[i]*funcion[i]){
			max = i;
			valMax = abs(funcion[i]);
		}
	}
	maximos[0] = max;
	valMax = 0;
	max = length/2;
	for (int i=length/2; i < length-1; i++){
		if (funcion[i] < width*255/10) 
			break;
		if (valMax < maximosLocales[i]*funcion[i]){
			max = i;
			valMax = abs(derivada2[i]);
		}
	}
	maximos[1] = max;
	return maximos;
}
void truncarHistograma(int * hist, int length,int threshold){
	for (int i = 0; i < length - 1; i++){
		if (hist[i] < threshold)
			hist[i] = 0;
	}
}